function M = calculate_M(q)
%@in:q角度，维度6*1
%@out:M项矩阵，维度6*6
q2=q(2);
q3=q(3);
q4=q(4);
q5=q(5);
t2 = cos(q2);
t3 = cos(q3);
t4 = cos(q4);
t5 = cos(q5);
t6 = sin(q3);
t7 = sin(q4);
t8 = sin(q5);
t9 = q2+q3;
t10 = q2+q5;
t11 = q3+q4;
t12 = q2.*2.0;
t13 = q3.*2.0;
t14 = q4.*2.0;
t15 = q5.*2.0;
t22 = -q5;
t16 = t5.^2;
t17 = cos(t9);
t18 = cos(t10);
t19 = cos(t11);
t20 = q4+t9;
t21 = q5+t9;
t23 = -t15;
t28 = q2+t22;
t30 = t9+t22;
t36 = t5./5.0e+3;
t39 = t4.*1.291571e+6;
t45 = t3.*3.4751112e-1;
t47 = t3.*t4.*6.4782432e-2;
t48 = t6.*t7.*6.4782432e-2;
t49 = t4.*1.16313912e-1;
t50 = t2.*2.06068104e-1;
t51 = t4.*5.8156956e-2;
t52 = t5.*5.483817e-3;
t59 = t7.*t8.*2.3248278e-2;
t61 = t3.*t7.*t8.*1.2948408e-2;
t62 = t4.*t6.*t8.*1.2948408e-2;
t63 = t7.*t8.*1.1624139e-2;
t24 = cos(t20);
t25 = cos(t21);
t26 = q5+t20;
t29 = cos(t28);
t31 = t15+t20;
t32 = cos(t30);
t33 = t20+t22;
t34 = t20+t23;
t40 = t19.*1.438712e+6;
t44 = t39+6.09313e+5;
t46 = t17.*9.3121968e-2;
t54 = t18.*6.474204e-3;
t55 = -t48;
t56 = -t50;
t57 = -t52;
t60 = t16.*2.042787e-3;
t27 = cos(t26);
t35 = cos(t31);
t37 = cos(t33);
t38 = cos(t34);
t53 = -t46;
t58 = -t54;
t64 = t24.*3.842787e-3;
t65 = -t60;
t68 = t29.*6.474204e-3;
t69 = t24.*2.6026281e-2;
t71 = t25.*5.8120695e-3;
t79 = t32.*5.8120695e-3;
t84 = t40+t44;
t86 = t5.*t44.*9.0e-9;
t41 = t27./1.0e+4;
t43 = t37./1.0e+4;
t66 = t27.*1.3015575e-4;
t67 = t27.*5.35366125e-3;
t72 = -t68;
t73 = -t69;
t74 = -t71;
t75 = t37.*1.3015575e-4;
t76 = t35.*5.1069675e-4;
t78 = t38.*5.1069675e-4;
t80 = t37.*5.35366125e-3;
t82 = -t79;
t87 = -t86;
t88 = t5.*t84.*9.0e-9;
t90 = t51+t63+t65+3.0014817e-2;
t94 = t45+t47+t49+t55+t59+t61+t62+t65+3.36806527e-1;
t42 = -t41;
t70 = -t66;
t77 = -t75;
t81 = -t78;
t83 = -t80;
t89 = -t88;
t93 = t47+t55+t61+t62+t90;
t85 = t42+t43;
t91 = t70+t73+t76+t81+t83;
t92 = t54+t64+t67+t71+t72+t77+t82;
t95 = t53+t74+t82+t91;
t96 = t56+t58+t72+t95;
M = reshape([t5.*1.0447011e-2+t19.*6.4782432e-2+t45+t51-cos(t9.*2.0+t14).*1.177531825e-2+cos(t9+t14+t21).*2.7419085e-3-cos(t9+t14+t30).*2.7419085e-3-cos(t9.*2.0).*1.52045855e-1-cos(t9.*2.0+t14+t15).*2.55348375e-4-cos(t9.*2.0+t14+t23).*2.55348375e-4-cos(q4+q5).*5.8120695e-3-cos(q2+t9).*3.4751112e-1-cos(q5+t11).*6.474204e-3-cos(q2+t20).*6.4782432e-2+cos(q4+t22).*5.8120695e-3-cos(t9+t20).*5.8156956e-2+cos(t10+t20).*6.474204e-3+cos(t11+t22).*6.474204e-3+cos(t20+t21).*5.8120695e-3-cos(t20+t28).*6.474204e-3-cos(t20+t30).*5.8120695e-3-cos(t12).*3.19610264e-1+cos(t15).*5.1069675e-4+6.3040101225e-1,t96,t95,t91,t92,t85,t96,t3.*6.9502224e-1+t49+t59+t65+t3.*t4.*1.29564864e-1-t6.*t7.*1.29564864e-1+t3.*t7.*t8.*2.5896816e-2+t4.*t6.*t8.*2.5896816e-2+9.84727055e-1,t94,t93,t89,t36,t95,t94,t49+t59+t65+3.36806527e-1,t90,t87,t36,t91,t93,t90,t8.^2.*2.042787e-3+2.797203e-2,t57,t36,t92,t89,t87,t57,3.842787e-3,0.0,t85,t36,t36,t36,0.0,2.0e-4],[6,6]);
